#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import serial
from std_msgs.msg import String
from std_msgs.msg import ByteMultiArray
from serial.tools import list_ports

class PySerialNode:
    def __init__(self):
        # 初始化ROS节点
        rospy.init_node('pyserial_node', anonymous=False)
        
        # 获取参数
        self.port = rospy.get_param('~port', '/dev/ttyUSB2')
        self.baudrate = rospy.get_param('~baudrate', 115200)
        self.timeout = rospy.get_param('~timeout', 1)

        # 自动检测可用串口
        if not self.port:
            available_ports = self.list_serial_ports()
            if available_ports:
                self.port = available_ports[0].device
                rospy.loginfo(f"Auto-detected serial port: {self.port}")
            else:
                rospy.logerr("No serial ports detected")
                rospy.signal_shutdown("No serial ports available")
                return
        
        # 尝试打开串口
        try:
            self.serial = serial.Serial(
                port=self.port,
                baudrate=self.baudrate,
                timeout=self.timeout
            )
            rospy.loginfo(f"Successfully opened serial port {self.port} at {self.baudrate} baud")
        except serial.SerialException as e:
            rospy.logerr(f"Failed to open serial port: {e}")
            rospy.signal_shutdown("Serial port error")
            return
        
        # 创建发布者和订阅者
        # self.pub = rospy.Publisher('serial_data', ByteMultiArray, queue_size=10)
        rospy.Subscriber('send_serial', String, self.serial_write)
        
    def list_serial_ports(self):
        """列出所有可用的串口"""
        return list(serial.tools.list_ports.comports())
    
    def serial_write(self, msg):
        """写入数据到串口"""
        if self.serial.is_open:
            try:
                self.serial.write((msg.data + '\n').encode('gbk'))  # 添加换行符并发送
                rospy.loginfo(f"Sent: {msg.data}")
            except serial.SerialException as e:
                rospy.logerr(f"Serial write error: {e}")
    
    def read_serial(self):
        """从串口读取数据"""
        if self.serial.is_open and self.serial.in_waiting > 0:
            try:
                data = self.serial.read_all()
                if data:
                    # msg = ByteMultiArray()
                    # msg.data = list(data)  # 将字节数据转换为列表
                    print(type(data))
                    print(data)
                    # self.pub.publish(msg)
                    # print(msg.data)
            except serial.SerialException as e:
                rospy.logerr(f"Serial read error: {e}")
            except UnicodeDecodeError as e:
                rospy.logerr(f"Decode error: {e}")
    
    def run(self):
        """主循环"""
        rate = rospy.Rate(10)  # 10Hz
        while not rospy.is_shutdown():
            self.read_serial()           
            rate.sleep()

if __name__ == '__main__':
    try:
        node = PySerialNode()
        node.run()
    except rospy.ROSInterruptException:
        pass
    finally:
        if hasattr(node, 'serial') and node.serial.is_open:
            node.serial.close()
            rospy.loginfo("Serial port closed")